#include "oberon7_robot_driver/oberon7.hpp"

namespace oberon7_robot_driver
{
  Oberon7Driver::Oberon7Driver() : Node("Oberon7DriverNode"),
    _joint_pos(6), _joint_vel(6), _joint_eff(6)
  {
    _azimuth_pub = this->create_publisher<Float64>(
      "/usv/arm/joint/azimuth/cmd_pos", 30);
    _shoulder_pub = this->create_publisher<Float64>(
      "/usv/arm/joint/shoulder/cmd_pos", 30);
    _elbow_pub = this->create_publisher<Float64>(
      "/usv/arm/joint/elbow/cmd_pos", 30);
    _roll_pub = this->create_publisher<Float64>(
      "/usv/arm/joint/roll/cmd_pos", 30);
    _pitch_pub = this->create_publisher<Float64>(
      "/usv/arm/joint/pitch/cmd_pos", 30);
    _wrist_pub = this->create_publisher<Float64>(
      "/usv/arm/joint/wrist/cmd_pos", 30);

    _state_sub = this->create_subscription<JointState>(
      "joint_states",
      10,
      std::bind(&Oberon7Driver::JointStatesCallback, this, std::placeholders::_1)
    );
  }


  void Oberon7Driver::JointStatesCallback(const JointState::SharedPtr msg) 
  {
    for (size_t i = 0; i < 6; i++)
    {
      _joint_pos[i] = msg->position[i];
      _joint_vel[i] = msg->velocity[i];
      _joint_eff[i] = msg->effort[i];
    }    
  }


  void Oberon7Driver::SendJointCommand(std::vector<double> joint_cmd)
  {
    auto azimuth_msg = Float64();
    azimuth_msg.data = joint_cmd[0];
    auto shoulder_msg = Float64();
    shoulder_msg.data = joint_cmd[1];
    auto elbow_msg = Float64();
    elbow_msg.data = joint_cmd[2];
    auto roll_msg = Float64();
    roll_msg.data = joint_cmd[3];
    auto pitch_msg = Float64();
    pitch_msg.data = joint_cmd[4];
    auto wrist_msg = Float64();
    wrist_msg.data = joint_cmd[5];
    
    _azimuth_pub->publish(azimuth_msg);
    _shoulder_pub->publish(shoulder_msg);
    _elbow_pub->publish(elbow_msg);
    _roll_pub->publish(roll_msg);
    _pitch_pub->publish(pitch_msg);
    _wrist_pub->publish(wrist_msg);
  }


  std::vector<double> Oberon7Driver::GetJointPosition()
  {
    return _joint_pos;
  }


  std::vector<double> Oberon7Driver::GetJointVelocity()
  {
    return _joint_vel;
  }


  std::vector<double> Oberon7Driver::GetJointEffort()
  {
    return _joint_eff;
  }
}